
using System.Collections.Generic;
using PEMath;


namespace PEPhysx
{
    public class PECylinderCollider : PEColliderBase
    {
        public PEInt mRadius;

        public PECylinderCollider(ColliderConfig cfg)
        {
            name = cfg.mName;
            mPos = cfg.mPos;
            mRadius = cfg.mRadius;
        }

        /**
         * 检测与其他碰撞体的碰撞，并返回矫正后速度与矫正偏移
         */
        public void CalcCollidersInteraction(List<PEColliderBase> colliders, ref PEVector3 velocity,
            ref PEVector3 borderAdjust)
        {
            if (velocity == PEVector3.zero)
                return;
            //检测后得到修正信息：
            var collisionInfos = new List<CollisionInfo>();
            var normal = PEVector3.zero;
            var adjust = PEVector3.zero;
            for (int i = 0; i < colliders.Count; i++)
            {
                if (DetectContact(colliders[i], ref normal, ref adjust))
                {
                    var colliderInfo = new CollisionInfo
                    {
                        collider = colliders[i],
                        normal = normal,
                        borderAdjust = adjust
                    };
                    collisionInfos.Add(colliderInfo);
                }
            }
            
            //处理修正信息：
            if (collisionInfos.Count == 1)
            {
                var info = collisionInfos[0];
                CorrectVelocity(ref velocity, info.normal);
                borderAdjust = info.borderAdjust;
            }
            else if(collisionInfos.Count > 1)
            {
                var nearestNormal = FindNearestNormal(collisionInfos,velocity);
                //如果速度方向在法线方向区外才处理，否则
                if (IsOutOfNormals(collisionInfos, velocity, nearestNormal))
                {
                    CorrectVelocity(ref velocity, nearestNormal);
                    var adjSum = PEVector3.zero;
                    for (int i = 0; i < collisionInfos.Count; i++)
                    {
                        adjSum += collisionInfos[i].borderAdjust;
                    }
                    borderAdjust = adjSum;
                }
                else
                    velocity = PEVector3.zero;
            }
            

        }
        //修正速度
        private void CorrectVelocity(ref PEVector3 velocity, PEVector3 normal) 
        {
            if(normal == PEVector3.zero) //基本不用
                return;
            
            var prjLen = PEVector3.Dot(velocity, normal);
            //确保是靠近，不是远离
            if(prjLen < 0)
                velocity += -prjLen * normal;//原速度+修正向量，就修正了速度
        }

        //找出速度方向离法线区最近的法线（与法线方向夹角最大的），此法线作为修正依据
        private PEVector3 FindNearestNormal(List<CollisionInfo> infoList ,PEVector3 velocity)
        {
            var maxAngleValue = new PEInt(1);//值越小角度越大
            var normal = PEVector3.zero;
            PEInt tempValue;
            for (int i = 0; i < infoList.Count; i++)
            {
                //找出速度方向与法线方向夹角最大的碰撞法线，速度校正由这个法线来决定
                tempValue = PEVector3.Dot(velocity, infoList[i].normal);
                if (tempValue < maxAngleValue)
                {
                    maxAngleValue = tempValue;
                    normal = infoList[i].normal;
                }
            }
            return normal;
        }
        
        //速度方向是否在法线方向区外
        private bool IsOutOfNormals(List<CollisionInfo> infoList,PEVector3 velocity,PEVector3 nearestNormal)
        {
            var centerNormal = PEVector3.zero;
            for (int i = 0; i < infoList.Count; i++)
            {
                centerNormal += infoList[i].normal;
            }
            centerNormal /= infoList.Count;
            
            var v2cDot = PEVector3.Dot(velocity, -centerNormal);
            var n2cMaxDot = PEInt.zero;
            PEInt tempDot;
            for (int i = 0; i < infoList.Count; i++)
            {
                tempDot = PEVector3.Dot(centerNormal, infoList[i].normal);
                n2cMaxDot = tempDot < n2cMaxDot ? tempDot : n2cMaxDot;
            }
            
            return v2cDot < n2cMaxDot;//角度大于一半
        }

        

        /**
         * 检测自己（圆）与BoxCollider的碰撞
         */
        public override bool DetectBoxContact(PEBoxCollider col, ref PEVector3 normal, ref PEVector3 borderAdjust)
        {
            //1.首先得到圆心离Box表面最近的接触点P：
            //得到box中心到圆心的距离向量在x，z轴方向的投影长度
            PEVector3 disOffset = mPos - col.mPos;
            disOffset.y = PEInt.zero;
            var dot_x = PEVector3.Dot(disOffset, col.mDir[0]);
            var dot_z = PEVector3.Dot(disOffset, col.mDir[2]);
            //判断圆的位置，若圆不接触Box，则返回
            if (PECalc.Abs(dot_x) >= col.mSize.x + mRadius || PECalc.Abs(dot_z) >= col.mSize.z + mRadius)
                return false;
            //计算接触点P离Box中心的轴向偏移
            PEInt clamp_x = PECalc.Clamp(dot_x, -col.mSize.x, col.mSize.x);
            PEInt clamp_z = PECalc.Clamp(dot_z, -col.mSize.z, col.mSize.z);
            //圆心若在box里,则需要将其移出
            PEInt dis_x = col.mSize.x - PECalc.Abs(dot_x);
            PEInt dis_z = col.mSize.z - PECalc.Abs(dot_z);
            var centreInBox = dis_x > 0 && dis_z > 0;
            if (centreInBox)
            {
                //离边界更近的为P点
                if (dis_x <= dis_z)
                    clamp_x = dot_x > 0 ? col.mSize.x : -col.mSize.x;
                else
                    clamp_z = dot_z > 0 ? col.mSize.z : -col.mSize.z;
            }
            //按公式计算接触点P：Box中心位置+轴向偏移
            PEVector3 point = col.mPos;
            point += clamp_x * col.mDir[0];
            point += clamp_z * col.mDir[2];

            //2.根据点p与圆心获取矫正信息（矫正方向和矫正向量）
            PEVector3 po = mPos - point;
            po.y = 0;
            if (centreInBox)
            {
                normal = -po.normalized;
                borderAdjust = normal * (mRadius + po.magnitude);
            }
            else
            {
                normal = po.normalized;
                borderAdjust = normal * (mRadius - po.magnitude);
            }
            return true;
        }
        
        /**
         * 检测自己（圆）与CylinderCollider的碰撞
         */
        public override bool DetectSphereContact(PECylinderCollider col, ref PEVector3 normal, ref PEVector3 borderAdjust)
        {
            PEVector3 disOffset = mPos - col.mPos;
            disOffset.y = 0;
            if(PEVector3.SqrMagnitude(disOffset) >= (col.mRadius + mRadius)*(col.mRadius + mRadius))
                return false;
            normal = disOffset.normalized;
            borderAdjust = normal * (mRadius + col.mRadius - disOffset.magnitude);
            return true;
        }
    }
}

